#include "joint_conf.h"
#include <cmath>
using namespace axisgroup;
using namespace std;

JointConf::JointConf(int c1, int c4, int c6, int cx) : cf1(c1), cf4(c4), cf6(c6), cfx(cx) {}
JointConf::JointConf(int cf[4]) : cf1(cf[0]), cf4(cf[1]), cf6(cf[2]), cfx(cf[3]) {}
bool JointConf::operator==(JointConf& cf)
{
    if (cf1 == cf.cf1 && cf4 == cf.cf4 && cf6 == cf.cf6 && cfx == cf.cfx)
        return true;
    return false;
}

int JointConf::judge_cfn(double t)
{
    int ret;

    if (t >= 0 && t < M_PI / 2)
        ret = 0;
    else if (t >= M_PI / 2 && t < M_PI)
        ret = 1;
    else if (t >= M_PI && t < M_PI * 1.5)
        ret = 2;
    else if (t >= 1.5 * M_PI && t < 2 * M_PI)
        ret = 3;
    else if (t >= 2 * M_PI)
        ret = 4;
    else if (t < 0 && t >= -M_PI / 2)
        ret = -1;
    else if (t < -M_PI / 2 && t >= -M_PI)
        ret = -2;
    else if (t < -M_PI && t >= -1.5 * M_PI)
        ret = -3;
    else if (t < -1.5 * M_PI && t >= -2 * M_PI)
        ret = -4;
    else if (t < -2 * M_PI)
        ret = -5;
    return ret;
}
